﻿using System;
using System.Collections.Generic;
using System.Numerics;

namespace SharpSoft.Maths
{
    /// <summary>
    /// 曲线工具类
    /// </summary>
    public class CurveHepler
    {
        static CurveHepler()
        {
        }

        /// <summary>
        /// 获取插值点(Catmull-Rom插值算法)
        /// </summary>
        /// <param name="points">输入点</param>
        /// <param name="tension">拉力取值-4到4</param>
        /// <param name="continuity">连续性</param>
        /// <param name="bias">误差</param>
        /// <param name="steplength">步长</param>
        /// <returns></returns>
        public static List<Vector2> GetInterpolatedPoints(Vector2[] points, float tension, float continuity, float bias, float steplength)
        {
            var interpolatedPoints = new List<Vector2>();
            if (points.Length < 3)
            {
                interpolatedPoints.AddRange(points);
            }
            else if (points.Length == 3)
            {
                var ps = GetInterpolatedPoints(points[0], points[0], points[1], points[2], tension, continuity, bias, steplength);
                interpolatedPoints.AddRange(ps);
                ps = GetInterpolatedPoints(points[0], points[1], points[2], points[2], tension, continuity, bias, steplength);
                interpolatedPoints.AddRange(ps);
            }
            else
            {
                for (int i = 0; i < points.Length; i++)
                {
                    if (i == 0)
                    {
                        var point1 = points[i];
                        var point2 = points[i];
                        var point3 = points[i + 1];
                        var point4 = points[i + 2];

                        var ps = GetInterpolatedPoints(point1, point2, point3, point4, tension, continuity, bias, steplength);
                        interpolatedPoints.AddRange(ps);
                    }
                    else if (i + 2 < points.Length)
                    {
                        var point1 = points[i - 1];
                        var point2 = points[i];
                        var point3 = points[i + 1];
                        var point4 = points[i + 2];

                        var ps = GetInterpolatedPoints(point1, point2, point3, point4, tension, continuity, bias, steplength);
                        interpolatedPoints.AddRange(ps);
                    }
                    else if (i + 1 < points.Length)
                    {
                        var point1 = points[i - 1];
                        var point2 = points[i];
                        var point3 = points[i + 1];
                        var point4 = points[i + 1];

                        var ps = GetInterpolatedPoints(point1, point2, point3, point4, tension, continuity, bias, steplength);
                        interpolatedPoints.AddRange(ps);
                    }
                }
            }


            return interpolatedPoints;
        }
        private static List<Vector2> GetInterpolatedPoints(Vector2 point1, Vector2 point2, Vector2 point3, Vector2 point4, float tension, float continuity, float bias, float steplength)
        {
            var interpolatedPoints = new List<Vector2>();

            var v = point3 - point2;
            var vlength = v.Length();
            for (var i = 0f; i <= vlength; i += steplength)
            {//TODO:将使用steps均分改成使用steplength将每段分成等长的N段
                var s = i / vlength;

                var h1 = 2f * MathF.Pow(s, 3f) - 3F * MathF.Pow(s, 2f) + 1f;
                var h2 = (-2f) * MathF.Pow(s, 3f) + 3f * MathF.Pow(s, 2f);
                var h3 = MathF.Pow(s, 3f) - 2f * MathF.Pow(s, 2f) + s;
                var h4 = MathF.Pow(s, 3f) - MathF.Pow(s, 2f);

                var tDix = (1f - tension) * (1f + continuity) * (1f + bias) * (point2.X - point1.X) / 2f + (1f - tension) * (1f - continuity) * (1f - bias) * (point3.X - point2.X) / 2f;
                var tDiy = (1f - tension) * (1f + continuity) * (1f + bias) * (point2.Y - point1.Y) / 2f + (1f - tension) * (1f - continuity) * (1f - bias) * (point3.Y - point2.Y) / 2f;

                var tSix = (1f - tension) * (1f - continuity) * (1f + bias) * (point3.X - point2.X) / 2f + (1f - tension) * (1f + continuity) * (1f - bias) * (point4.X - point3.X) / 2f;
                var tSiy = (1f - tension) * (1f - continuity) * (1f + bias) * (point3.Y - point2.Y) / 2f + (1f - tension) * (1f + continuity) * (1f - bias) * (point4.Y - point3.Y) / 2f;

                var x = h1 * point2.X + h2 * point3.X + h3 * tDix + h4 * tSix;
                var y = h1 * point2.Y + h2 * point3.Y + h3 * tDiy + h4 * tSiy;

                interpolatedPoints.Add(new Vector2(x, y));
            }

            interpolatedPoints.Add(point3);

            return interpolatedPoints;
        }
    }
}
